#include "Process.hpp"
#include "../yolov8/src/funtion.hpp"
#include "SerialPort.hpp"
#include "Draw_Contours.hpp"
#include <iomanip>  // 需要包含这个头文件用于std::setw, std::setfill

// 全局变量定义
SerialPort serial; 
SerialPort::Config config; // 串口
// int tools::g_threshold = 127; // 全局阈值
// bool tools::trackbar_created = false;

namespace process{
    int isNullFlagCount = 5; 

    Mat get_armor(Mat& image) {
        //画面处理
        Mat binary;
        cvtColor(image, binary, COLOR_BGR2GRAY); // 转灰度

        threshold(binary,binary,120,255,THRESH_BINARY); // 100 / 190 / 250

        // 滑动条测试来调参
        // tools::myframeBar(binary); // 190 这个数值可以通过这里来调节
        // imshow("threshold_img_binary",binary);

        // 提取灯条轮廓
        vector<vector<Point>>contours;
        findContours(binary, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE); // 圈起来
        
        list<Lightbar>lightbars;
        for (const auto& contour : contours) { 
            if (contour.size() < 15) continue; // 跳过点数不足的轮廓
            cv::RotatedRect rectangle = cv::minAreaRect(contour);
            Lightbar lightbar(rectangle); // 框选灯条
            // tools::draw_contours(image, contour);//一个个轮廓进行绘画 test
            // 颜色判断
            lightbar.color = ArmorPatten::Color_Dcide(image,contour,ENEMYCOLOR); 
            if(lightbar.color == ArmorPatten::Color::black)continue;

            // 条件判断
            if(!lightbar.lightbar_decide())continue;

            // 将符合条件的灯条显示出参数
            circle(image,lightbar.topPoint,5,Scalar(50,255,0),-1);
            circle(image,lightbar.bottomPoint,5,Scalar(50,255,0),-1);
            // putText(image,fmt::format("ratio{: .3f}",lightbar.ratio),lightbar.topPoint+Point2f(0,-30),FONT_HERSHEY_SIMPLEX,1,Scalar(250,100,55),1);

            // 添加灯条
            lightbars.push_back(lightbar);
        }
        /* 显示框选结果 
        // imshow("框选灯条",image);
        */


        // 灯条排序 从左到右
        lightbars.sort([](const Lightbar &a ,const Lightbar &b){
            return a.midPoint.x < b.midPoint.x;
        });

        // 开始对装甲板筛选
        select(lightbars,image);
        
        // max_distance
        list<KalmanTracker>::iterator nearestIt;
        // 数据包
        std::vector<uint16_t> values = {};


        // 找出最近的
        if(!frameArmors.empty()){

            nearestIt = frameArmors.begin();
            // 寻找出最近距离的tracker
            for(list<KalmanTracker>::iterator it = std::next(nearestIt);it!=frameArmors.end();it++){
                if((*it).frameArmor.armorDistance < (*nearestIt).frameArmor.armorDistance){
                nearestIt = it ;
                }
            }
            if((*nearestIt).frameArmor.firePoint != Point2f(0,0)){ 
                circle(image,(*nearestIt).frameArmor.firePoint,5,Scalar(50,150,50),-1); // firePoint 
                // 串口发送预处理
                // float x = (*nearestIt).predictedPoint.x; 
                // float y = (*nearestIt).predictedPoint.y;
                // int hexX = static_cast<int>(x);
                // int hexY = static_cast<int>(y);
                // hexX &= 0xFFFF;
                // hexY &= 0XFFFF;
                // std::vector<uint16_t> values = {};
                // values.push_back(hexX);
                // values.push_back(hexY);

                // 串口发送预处理
                // 1. 获取OpenCV相机坐标系下的坐标
                double x_cv = (*nearestIt).p_tvec.at<double>(0); // 右X
                double y_cv = (*nearestIt).p_tvec.at<double>(1) ; // 下Y  - 0.085 
                double z_cv = (*nearestIt).p_tvec.at<double>(2); // 前Z
                // 2. 坐标系转换：OpenCV → 机器人/电控坐标系
                // 转换矩阵：[0  0  1; -1  0  0; 0 -1  0]
                // double x_robot = z_cv;      // 前X
                double x_robot = z_cv;      // 前X
                double y_robot = -x_cv;     // 左Y  
                double z_robot = -y_cv;     // 上Z
                
                // 计算炮口yaw轴需要的角度
                double angle_yaw = atan2(y_robot, x_robot);  // 弧度制
                double yaw_deg = angle_yaw * 180.0 / M_PI + 1;      // 转换为角度制

                // 计算炮口pitch轴需要的角度
                double angle_pitch = atan2(z_robot, x_robot );  // 弧度制
                double pitch_deg = angle_pitch * 180.0 / M_PI;   // 转换为角度制

                int yaw_int = static_cast<int>(yaw_deg * 100.0); 
                int pitch_int = static_cast<int>(pitch_deg * 100.0);

                // 限制范围在 9000 到 -9000
                yaw_int = std::max(-9000, std::min(9000, yaw_int)) + 9000;
                pitch_int = std::max(-9000, std::min(9000, pitch_int)) + 9000;

                yaw_int &= 0xFFFF;
                pitch_int &= 0XFFFF;
                values.push_back(yaw_int);
                values.push_back(pitch_int);
                putText(image,"x: "+to_string(x_robot)+" y: "+to_string(y_robot)+" z: "+to_string(z_robot),
                Point2f(10,30),FONT_HERSHEY_SIMPLEX,1,Scalar(10,210,255),2.5);

                putText(image,"yaw: "+to_string(yaw_deg)+" pitch: "+to_string(pitch_deg)+" ",
                Point2f(10,60),FONT_HERSHEY_SIMPLEX,1,Scalar(250,100,55),2.5);
                // test
                // putText(image,"yaw_int :"+std::to_string(yaw_int),(*nearestIt).frameArmor.left.topPoint,FONT_HERSHEY_SIMPLEX,1,Scalar(100,100,55),3);
                // putText(image,"pitch_int :"+std::to_string(pitch_int),(*nearestIt).frameArmor.right.topPoint,FONT_HERSHEY_SIMPLEX,1,Scalar(100,100,55),3);
                
                isNullFlagCount = 5;
            }
        // 51 28 23 28 23 E7 
        }else if(frameArmors.empty() ){ 
            int YawAndPitch_int = 9000;
            YawAndPitch_int &= 0xFFFF;
            values.push_back(YawAndPitch_int);
            values.push_back(YawAndPitch_int);
            if(isNullFlagCount == 0){
                return image;
            }else{
                isNullFlagCount--;
            }
            cout<<"丢失装甲板count倒数："<<isNullFlagCount<<endl;
        }

        if (serial.isOpen() && !values.empty()) {
            std::vector<uint8_t> bytes = serial.uint16ArrayToLittleEndian(values);
            bytes.insert(bytes.begin() , 0x51);
            serial.writeWithChecksum(bytes);

        } 
        // else {
        //     std::cout << "串口打开失败: " << serial.getLastError() << std::endl;
        // }  

        return image;// 这里会返回两次？
    }



    void select(const list<Lightbar> &lightbars,Mat &bgr_image){
        armors.clear(); 
        for(auto left = lightbars.cbegin();left != lightbars.cend();left++){
            for(auto right = next(left);right != lightbars.cend();right++){
                // 颜色判断
                if((*left).color != (*right).color)continue;
                Armor armor(*left,*right,bgr_image);

                // 比例判断 
                if(armor.ratio_min <= 2 || armor.ratio_max >= 3){ // 3.9
                    // cout<<"ration_error: "<<armor.ratio_min
                    // <<"ration_error: "<<armor.ratio_max<<endl;
                continue;
                }
                            
                // 角度判断 
                if(armor.errorAngleBar*57.3 >= 15){ 
                    // cout<<"errorAngleBar_error: "<<armor.errorAngleBar*57.3<<endl;
                    continue;
                }

                // 两边灯条之间的比例 
                if(armor.side_ratio*100 >= 160){ 
                    // cout<<"side_ratio_error: "<<armor.side_ratio*100<<endl;
                    continue;
                }

                // 装甲板平行判断 
                if(abs(armor.armorAngle - (*left).error_angle)*57.3 >=20 || abs(armor.armorAngle - (*right).error_angle)*57.3 >=20  ){ // 24
                    // cout<<"armorAngle_erro_left :"<<abs(armor.armorAngle - (*left).error_angle)*57.3<<
                    // "armorAngle_erro_left: "<<abs(armor.armorAngle - (*right).error_angle)*57.3<<endl;
                    continue;
                }
                        
                // 截取ROI 
                // armor.get_pattern(bgr_image,*left,*right); 

                // armor.Name_Dcide();
                
                // 判断该机器人是几号机器人 并 将分类ID映射到ArmorPatten::Name
                /*switch (funtion(armor.roiImg)) {
                case 0:
                    armor.robotName = ArmorPatten::Name::black; // 空
                    break;
                case 1:
                    armor.robotName = ArmorPatten::Name::hero; // 英雄
                    break;
                case 2:
                    armor.robotName = ArmorPatten::Name::sentry; // 哨兵
                    break;
                case 3:
                    armor.robotName = ArmorPatten::Name::infantry; // 步兵
                    break;
                default:
                    armor.robotName = ArmorPatten::Name::black; // 默认为空
                    break;
                } //  switch end
                */

                // if(armor.robotName == ArmorPatten::Name::black){ // 报错提醒
                //     // putText(bgr_image,"(6) patten error",Point2f(10,820),FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,250,90),2);
                //     continue;
                // }

                // 执行到这里说明装甲板已经被添加进去了
                armors.emplace_back(armor); 
                
                    
            }
        }
        
        
        // thread1

        // 检查容器是否为空
        if (armors.empty()){
            frameArmors.clear();// 清空操作
            return ;// 提前结束,return默认构造armor
        }

        // 每帧拍到的装甲板数量遍历
        for(int i = 0;i<armors.size();i++){
            
            tools::draw_armor(bgr_image, armors[i].corners);
            // 目标装甲板显示三维信息并计算目标装甲板的firePoint、armorDistance 和 rvec/tvec
            armors[i].armorPose(bgr_image);
            // 构造追踪器 (这里做初始化)
            KalmanTracker kalmantracker(armors[i]); 

            // ------------这里就是简单的为装甲板上个基础的追踪-------------
            // 装甲板追踪操作
            if(frameArmors.empty()){ // frameNumbers 为空的时候就直接添加
                kalmantracker.inital_KalmanTracker(armors[i].rvec,armors[i].tvec); // 初始化操作
                frameArmors.push_back(kalmantracker); //深拷贝添加
            } else {
                // 匹配标志
                bool matched = false;
                // 开始寻找相匹配的装甲板
                for(list<KalmanTracker>::iterator it = frameArmors.begin();it!=frameArmors.end();it++){ // （写算法的地方，还有很大的优化空间）

                    // 上一个预测点 - 原始点
                    if( norm((*it).frameArmor.firePoint - armors[i].firePoint) > 180){
                        // cout<<"与上一个画面的装甲板距离相距："<<norm((*it).frameArmor.firePoint - armors[i].firePoint)<<" m"<<endl;
                        continue; // 相隔太远了可能性小
                    }
                    // 图案判断
                    // if((*it).frameArmor.robotName != armors[i].robotName ){
                        // cout<<"图案不匹配"<<endl;
                        // 可能还要优化
                        // cout<<int((*it).frameArmor.robotName)<<" != "<<int(armors[i].robotName)<<endl;
                        // imshow("(*it).img",(*it).frameArmor.roiImg);
                        // imshow("armors[i].img",armors[i].roiImg);
                        // continue;
                    // }
                    // 向量，区域，图案，距离
                    // ....


                    // 找到相匹配的装甲板做更新操作 
                    (*it).update_KalmanTracker(kalmantracker);
                    // 卡尔曼递归 
                    (*it).CumulateKalman();
                    matched = true;

                }
                // 如果没有找到就新增
                if(!matched){
                    kalmantracker.inital_KalmanTracker(armors[i].rvec,armors[i].tvec); // 初始化操作
                    frameArmors.push_back(kalmantracker);
                }

            }// end of it
            // -----------------------------------
        
        }// 遍历玩armors后

        // 每帧都要进行的frameArmor倒计时删除操作（3帧数容错）
        for(list<KalmanTracker>::iterator it = frameArmors.begin();it!=frameArmors.end();){
            if((*it).frameLostCount > 0){
                (*it).frameLostCount--;
                it++;
            }else{
                it = frameArmors.erase(it); // 删除
            }
        }
        
    }// 此时frameArmor就是带有预测的真装甲板了


}
